#include "Convolution2.h"
#include "MinkowskiOperation2.h"
#include "Allocator.h"
#include <boost/ref.hpp>


////////////////////////////////////////////////////
ConvolutionExImEdge2::ConvolutionExImEdge2(
	const ILinearEdge2& edgeA, const Vector2& tA, 
	const IImplicitBounded2& domainB, const Vector2& tB,
	IStackAllocator& stack)
: edgeA_(&edgeA), tA_(tA), domainB_(&domainB), tB_(tB), stack_(&stack)
{
}

ConvolutionExImEdge2::~ConvolutionExImEdge2()
{
}

const Vector2 ConvolutionExImEdge2::startPoint() const
{
	return transformedSupportPoint(*domainB_, tB_, edgeA_->startNormal()) + (edgeA_->startPoint() + tA_);
}

const Vector2 ConvolutionExImEdge2::endPoint() const
{
	return transformedSupportPoint(*domainB_, tB_, edgeA_->endNormal()) + (edgeA_->endPoint() + tA_);
}

const Vector2 ConvolutionExImEdge2::startNormal() const
{
	return edgeA_->startNormal();
}

const Vector2 ConvolutionExImEdge2::endNormal() const
{
	return edgeA_->endNormal();
}

////////////////////////////////////////////////////

class ConvolutionExImSegmentator2 : public IVertexIterator2
{
public:
	ConvolutionExImSegmentator2(
		const ILinearEdge2& edgeA, const Vector2& tA,
		const IImplicitBounded2& domainB, const Vector2& tB,
		Scalar tolerance, IStackAllocator& stack);
	~ConvolutionExImSegmentator2();


	virtual const Vector2 point() const;
	virtual const Vector2 normal() const;
	virtual bool next();
private:
	static const std::size_t N = 2;

	IEdge2* startEdgeB_;
	IEdge2* endEdgeB_;
	Vector2 tB_;
	Vector2           pointA_[N];
	IVertexIterator2* segmentatedEdgeB_[N];
	int currentIndex_;
	IStackAllocator*  stack_;
};


ConvolutionExImSegmentator2::ConvolutionExImSegmentator2(
		const ILinearEdge2& edgeA, const Vector2& tA,
		const IImplicitBounded2& domainB, const Vector2& tB,
		Scalar tolerance, IStackAllocator& stack)
: startEdgeB_()
, endEdgeB_()
, tB_(tB)
, currentIndex_(0)
, stack_(&stack)
{
	assert(!edgeA.normal().isZero());

	/** @todo remove vertex creation after modified trim() */
	const Vector2 n1 = edgeA.startNormal();
	const Vector2 n2 = edgeA.normal();
	const Vector2 n3 = edgeA.endNormal();

	//1. fix the position of A to start point.
	startEdgeB_ = domainB.trim(n1, n2, stack);
	pointA_[0] = edgeA.startPoint() + tA;
	segmentatedEdgeB_[0] = startEdgeB_->segmentate(tolerance, stack);

	//2. fix the normal shared
	//... construct line segment implicitly.
	
	//3. fix the position of shape 1
	endEdgeB_ = domainB.trim(n2 ,n3 , stack);
	pointA_[1] = edgeA.endPoint() + tA;
	segmentatedEdgeB_[1] = endEdgeB_->segmentate(tolerance, stack);
}

ConvolutionExImSegmentator2::~ConvolutionExImSegmentator2()
{
	stack_->destroy(segmentatedEdgeB_[1]);
	stack_->destroy(endEdgeB_);
	stack_->destroy(segmentatedEdgeB_[0]);
	stack_->destroy(startEdgeB_);
}

const Vector2 ConvolutionExImSegmentator2::point() const
{
	assert(0 <= currentIndex_ && currentIndex_ < N);
	assert(segmentatedEdgeB_[currentIndex_]);
	return ((segmentatedEdgeB_[currentIndex_]->point() + tB_) + pointA_[currentIndex_]);
}

const Vector2 ConvolutionExImSegmentator2::normal() const
{
	assert(0 <= currentIndex_ && currentIndex_ < N);
	assert(segmentatedEdgeB_[currentIndex_]);
	return (segmentatedEdgeB_[currentIndex_]->normal());
}

bool ConvolutionExImSegmentator2::next()
{
	assert(0 <= currentIndex_ && currentIndex_ < N);
	if (segmentatedEdgeB_[currentIndex_]->next())
	{
		return true;
	}
	else
	{
		if (currentIndex_ < N-1)
		{
			++currentIndex_;
			return true;
		}
		else
		{
			return false;
		}
	}
}


IVertexIterator2* ConvolutionExImEdge2::segmentate(Scalar tolerance, IStackAllocator& stack) const
{
	return stack.create<ConvolutionExImSegmentator2>(*edgeA_, tA_, *domainB_, tB_, tolerance, boost::ref(stack));
}

////////////////////////////////////////////////////
ConvolutionExIm2::ConvolutionExIm2(
		const IExplicitBounded2& domainA, const Vector2& tA,
		const IImplicitBounded2& domainB, const Vector2& tB,
		IStackAllocator& stack)
: boundaryA_(domainA.boundary(stack))
, tA_(tA)
, domainB_(&domainB)
, tB_(tB)
, stack_(&stack)
, currentEdge_(createCurrentEdge())
{
}

ConvolutionExImEdge2* ConvolutionExIm2::createCurrentEdge()
{
	return stack_->create<ConvolutionExImEdge2>(boundaryA_->current(), tA_, *domainB_, tB_, boost::ref(*stack_));
}

ConvolutionExIm2::~ConvolutionExIm2()
{
	stack_->destroy(currentEdge_);
	stack_->destroy(boundaryA_);
}

const IEdge2& ConvolutionExIm2::current() const
{
	assert(currentEdge_);	//valid if next() returns true and successed createCurrentEdge()
	return *currentEdge_;
}

bool ConvolutionExIm2::next()
{
	stack_->destroy(currentEdge_);
	bool hasNext = boundaryA_->next();
	//skip degenerated edge of boundary A
	while (hasNext)
	{
		if (!boundaryA_->current().normal().isZero())
			break;
		else
			hasNext = boundaryA_->next();
	}
	if (hasNext)
	{
		currentEdge_ = createCurrentEdge();
		return true;
	}
	else
	{
		return false;
	}
}
